#ifndef __MATRIX4_H
#define __MATRIX4_H

#include<iostream>
#include <cmath>
#include "vertex.h"

//#define M_PI 3.1415926535897932384626433832795

/* TODO Change Vertex TO 3D Vector
 */

class matrix4x4_t
{
public:
	matrix4x4_t() {};
	matrix4x4_t(const double init);
	matrix4x4_t(const matrix4x4_t & source);
	matrix4x4_t(const double source[4][4]);
	~matrix4x4_t() {};
	/*! attention, this function can cause the matrix to become invalid!
		unless you are sure the matrix is invertible, check invalid() afterwards! */
	matrix4x4_t & inverse();
	matrix4x4_t & transpose();
	void identity();
	void translate(double dx,double dy,double dz);
	void rotateX(double degrees);
	void rotateY(double degrees);
	void rotateZ(double degrees);
	void scale(double sx, double sy, double sz);
	int invalid() const { return _invalid; }
	const double * operator [] (int i) const { return matrix[i]; }
	double * operator [] (int i) { return matrix[i]; }

	// only used for rotation matrix in mesh, sets matrix[i] from a vector (elem0-2) and a double (elem3)
	// maybe a bit clumsy this
	void setRow(int i, const Vertex &v, double e3) { matrix[i][0]=v.x;  matrix[i][1]=v.y;  matrix[i][2]=v.z;  matrix[i][3]=e3; }
	void setColumn(int i, const Vertex &v, double e3) { matrix[0][i]=v.x;  matrix[1][i]=v.y;  matrix[2][i]=v.z;  matrix[3][i]=e3; }

protected:

	double  matrix[4][4];
	int _invalid;
};

inline matrix4x4_t  operator * (const matrix4x4_t &a,const matrix4x4_t &b)
{
	matrix4x4_t aux;
	
	for(int i=0;i<4;i++)
		for(int k=0;k<4;k++)
		{
			aux[i][k]=0;
			for(int j=0;j<4;j++)
				aux[i][k]+=a[i][j]*b[j][k];
		}
	return aux;
}

inline Vertex  operator * (const matrix4x4_t &a, const Vertex &b)
{
	return Vertex(a[0][0]*b.x+a[0][1]*b.y+a[0][2]*b.z,
										a[1][0]*b.x+a[1][1]*b.y+a[1][2]*b.z,
										a[2][0]*b.x+a[2][1]*b.y+a[2][2]*b.z);
}
/*
inline point3d_t  operator * (const matrix4x4_t &a, const point3d_t &b)
{
	return  point3d_t(a[0][0]*b.x+a[0][1]*b.y+a[0][2]*b.z+a[0][3],
										a[1][0]*b.x+a[1][1]*b.y+a[1][2]*b.z+a[1][3],
										a[2][0]*b.x+a[2][1]*b.y+a[2][2]*b.z+a[2][3]);
}
*/
#endif
